#include <cmath>
#include <string>
#include <iostream>
#include "FormationSlot.h"
using namespace std;

FormationSlot::FormationSlot(double dx, double dy, double rotation, FormationSlot& parent, int spotIndex){
    this->spotIndex = spotIndex;
    this->globalOrientation = Vector2D(1,0);
    this->globalPosition = Point2D(0,0);
    this->relativePosition = Vector2D(dx,dy);
    this->relativeOrientation = Vector2D(cos(rotation),sin(rotation));
    this->parent = &parent;
}

FormationSlot::FormationSlot(){
    this->spotIndex = 0;
    this->globalOrientation = Vector2D(1,0);
    this->globalPosition = Point2D(0,0);
    this->relativePosition = Vector2D(0,0);
    this->relativeOrientation = Vector2D(0,0);
    this->parent = NULL;
}

FormationSlot::FormationSlot(FormationSlot& fs) {
    this->spotIndex = fs.spotIndex;
    this->globalOrientation = fs.globalOrientation;
    this->globalPosition = fs.globalPosition;
    this->relativePosition = fs.relativePosition;
    this->relativeOrientation = fs.relativeOrientation;
    this->parent = fs.parent;
}

FormationSlot& FormationSlot::operator=(FormationSlot& fs) {
    //if (this->parent) delete this->parent;
    this->spotIndex = fs.spotIndex;
    this->globalOrientation = fs.globalOrientation;
    this->globalPosition = fs.globalPosition;
    this->relativePosition = fs.relativePosition;
    this->relativeOrientation = fs.relativeOrientation;
    this->parent = fs.parent;
    return *this;
}

FormationSlot::~FormationSlot() {

}

Point2D FormationSlot::getGlobalPosition() {
    Point2D pos;
    getGlobalPositionOrientation(pos);
    return pos;
}

double FormationSlot::getGlobalPositionOrientation(Point2D& position) {    
    cout<<this<<endl;
    if (this->parent) {
        double angle = this->parent->getGlobalPositionOrientation(position);
        Vector2D myPosition(this->relativePosition);
        turnVector(myPosition, angle);
        position += myPosition.toPointF();
        return angle;

    } else {
        position = this->globalPosition;

        return signedAngle(this->globalOrientation, Vector2D(1,0));
    }

}

void FormationSlot::setGlobalPosition(double x, double y) {
    if (!this->parent) {
       this->globalPosition.setX(x);
       this->globalPosition.setY(y);
    }
}


Vector2D FormationSlot::getGlobalOrientation() {
    if (this->parent) {
       double parentAngle = signedAngle(this->parent->getGlobalOrientation(),Vector2D(1,0));
       Vector2D myOrientation(this->relativeOrientation);
       double delta = signedAngle(myOrientation,Vector2D(1,0));
       double angle = parentAngle+delta;
       return Vector2D(cos(angle),sin(angle));
    }
    return Vector2D(this->globalOrientation);
}

void FormationSlot::setGlobalOrientation(double x, double y) {
     if (!this->parent) {
        this->globalOrientation.setX(x);
        this->globalOrientation.setY(y);
        if (this->globalOrientation.length()>0) this->globalOrientation.normalize();
     }
}

Vector2D FormationSlot::getRelativePosition() {
   return Vector2D(this->relativePosition);
}

double FormationSlot::getRelativeOrientation() {
  return signedAngle(Vector2D(this->relativeOrientation),Vector2D(1,0));
}

FormationSlot* FormationSlot::getSpotParent() {
    return this->parent;
}

int FormationSlot::getSpotIndex() {
   return this->spotIndex;
}
